package it.unicam.cs.cosy.bioshape.core.collisiondetection.broadphase;

import it.unicam.cs.cosy.bioshape.core.components.Entity;
import it.unicam.cs.cosy.bioshape.core.components.EntityPair;
import it.unicam.cs.cosy.bioshape.core.components.WallEntity;
import it.unicam.cs.cosy.bioshape.core.shapes.BoundingBox;

import java.util.ArrayList;
import java.util.Collections;
import java.util.Comparator;
import java.util.List;

import javax.vecmath.Point3d;

/**
 * Sweep and Prune implementation of the {@link BroadPhase} interface. Sweep and
 * Prune is especially effective in taking advantage of temporal coherence, i.e.
 * the fact that a physical configurations changes only slightly during one
 * single time-step. Thus, it is not suitable for simulation in which entities
 * are moving in particular nasty ways.
 * <p>
 * 
 * This implementation exploits ideas from several sources, especially Ericson's
 * book "Real-time Collision Detection". It is an array-based implementation
 * which relies on a sorting pass, each time the algorithm is applied. The worst
 * case performance is given by the sorting algorithm, the Java build-in sort
 * algorithm which is a merge-sort, guaranteed to have a worst-case performance
 * of <i>O(n log n)</i>.
 * 
 * @author Federico Buti
 * @see BroadPhase
 */
public class SAPBroadPhase implements BroadPhase {

	/**
	 * Reference to the {@code Entity} {@code List} of the associated
	 * {@code LocalCoordinator}
	 */
	private List<Entity> entities;
	/** Set of possibly overlapping pairs of entities */
	private List<EntityPair> overlappingPairs;
	/** Sorting axis for the current iteration */
	private int sortAxis = 0;
	/**
	 * Set of possibly overlapping pairs of entities
	 * 
	 * private List<EntityPair> overlappingPairs;
	 * 
	 * /** Aiding points to calculate coordinates variance
	 */
	private Point3d s1 = new Point3d();
	private Point3d s2 = new Point3d();
	private Point3d s3 = new Point3d();

	/** Comparator for the sorting */
	private AxisComparator comparator;

	/**
	 * Create a new Sweep & Prune broad phase instance. The passed values is
	 * used to construct the array of managed entities.
	 * 
	 * @param entities
	 *            the list of entities to be checked by the broad phase
	 */
	public SAPBroadPhase(List<Entity> entities) {
		this.entities = entities;
		overlappingPairs = new ArrayList<EntityPair>(entities.size() / 2
				- (entities.size() / 10));
		comparator = new AxisComparator();
		s1 = new Point3d();
		s2 = new Point3d();
		s3 = new Point3d();
	}

	/**
	 * Realizes the whole BroadPhase. Bounding boxes are maintained in an order
	 * array and sorted at each iteration through a {@code Comparator} object.
	 * Ordering is based on an axis coordinate chosen to be the one with the
	 * highest variance.
	 * 
	 * @return The {@code List} of overlapping couples of entities
	 */
	@Override
	public List<EntityPair> getOverlappingPairs() {
		overlappingPairs.clear();
		comparator.axis = sortAxis;
		s1.set(0, 0, 0);
		s2.set(0, 0, 0);
		s3.set(0, 0, 0);
		//
		Collections.sort(entities, comparator);
		//
		int size = entities.size();
		for (int i = 0; i < size; i++) {
			// get the center
			Point3d p = entities.get(i).getBounds().getCenter();
			// Update sum and sum2 for computing variance of centers
			s1.x += p.x;
			s2.x += p.x * p.x;
			s1.y += p.y;
			s2.y += p.y * p.y;
			s1.z += p.z;
			s2.z += p.z * p.z;
			//
			// Test collisions against all possible overlapping bounds following
			// current one
			for (int j = i + 1; j < size; j++) {
				// Stop when tested AABBs are beyond the end of current bounds
				if (entities.get(j).getBounds().getMin(sortAxis) > entities
						.get(i).getBounds().getMax(sortAxis))
					break;
				// else, checking...
				if (entities.get(i).getBounds()
						.intersect(entities.get(j).getBounds())) {
					if (!(entities.get(i) instanceof WallEntity && entities
							.get(j) instanceof WallEntity))
						overlappingPairs.add(new EntityPair(entities.get(i),
								entities.get(j)));
				}

			}
		}
		s3.x += s2.x - s1.x * s1.x / size;
		s3.y += s2.y - s1.y * s1.y / size;
		s3.z += s2.z - s1.z * s1.z / size;
		// Update axis sorted to be the one with greatest bounds variance
		sortAxis = 0;
		double temp = s3.x;
		if (s3.y > temp) {
			sortAxis = 1;
			temp = s3.y;
		}
		if (s3.z > temp)
			sortAxis = 2;
		//
		return overlappingPairs;
	}

	/**
	 * Used to confront two bounding boxes. Confrontation is only done over the
	 * selected axis of the current iteration. Thus, even if two boxes are equal
	 * according to the comparator they can differ for the other coordinates.
	 * 
	 * Note: this comparator imposes orderings that are inconsistent with
	 * equals.
	 * 
	 * @author Federico Buti
	 */
	class AxisComparator implements Comparator<Entity> {
		int axis;

		@Override
		public int compare(Entity e1, Entity e2) {
			if (e1.getBounds().getMin(axis) < e2.getBounds().getMin(axis))
				return -1;
			if (e1.getBounds().getMin(axis) > e2.getBounds().getMin(axis))
				return 1;
			return 0;
		}
	}

	// /**
	// * @param args
	// * arguments de che? :P
	// */
	// @SuppressWarnings("boxing")
	// public static void main(String args[]) {
	// ArrayList<BoundingBox> ab = new ArrayList<BoundingBox>(10000000);
	// Random r = new Random();
	// for (int i = 0; i < 8000000; i++) {
	// ab.add(new BoundingBox(new Point3d(r.nextDouble(), r.nextDouble(),
	// r.nextDouble()), r.nextDouble()));
	// }
	// System.out.println("Done!");
	// Long start = System.currentTimeMillis();
	// Collections.sort(ab, new Comparator<BoundingBox>() {
	// @Override
	// public int compare(BoundingBox bb1, BoundingBox bb2) {
	// if (bb1.getMin(0) < bb2.getMin(0))
	// return -1;
	// if (bb1.getMin(0) > bb2.getMin(0))
	// return 1;
	// return 0;
	// }
	// });
	// System.out.println(" --> " + (System.currentTimeMillis() - start));
	// for (int j = 0; j < 10; j++) {
	// System.out.println(ab.get(j).getMin(0));
	// }
	// }

	@SuppressWarnings("unused")
	private void quicksort(ArrayList<BoundingBox> t, int l, int r) {
		int i = l - 1, j = r;
		BoundingBox v = t.get(r);
		if (r <= l)
			return;
		for (;;) {
			while (t.get(++i).getMin(sortAxis) < v.getMin(sortAxis)) {
				// EMPTY box
			}

			while (v.getMin(sortAxis) < t.get(--j).getMin(sortAxis))
				if (j == l)
					break;
			if (i >= j)
				break;
			BoundingBox temp = t.get(i);
			t.set(i, t.get(j));
			t.set(j, temp);
		}
		BoundingBox temp = t.get(i);
		t.set(i, t.get(r));
		t.set(r, temp);
		//
		quicksort(t, l, i - 1);
		quicksort(t, i + 1, r);
	}
}